//
//  Rover.m
//  SSE
//
//  Created by Matt Roman on 10/11/07.
//  Copyright 2007 __MyCompanyName__. All rights reserved.
//

#import "Rover.h"

@implementation Rover

- (id)init
{
	[super init];
	bzero(&robot, sizeof(roverProperties));
	robot.wp = malloc(sizeof(wayPoint));
	bzero(robot.wp, sizeof(wayPoint));
	
	robot.voltage = 12.2;
	robot.electronicsCurrent = 1.2;
	robot.panelCurrent = 1.2;
	robot.motorCurrent = 1.2;
	robot.state = RSAvoidingNearbyObstacles;
	bzero(&bodyLaser, sizeof(bodyLaser));
	bzero(&panelLaser, sizeof(panelLaser));
	bzero(&profileLaser, sizeof(profileLaser));
	
	[self testLaser:&bodyLaser type:SenseBodyLaser];
	[self testLaser:&panelLaser type:SensePanelLaser];
	[self testLaser:&profileLaser type:SenseProfileLaser];
	
	return self;
}

- (roverProperties *)getRobotProperties
{
	return &robot;
}

- (void)setRoverPosition:(Point3D)pos
{
	robot.position = pos;
}

- (Point3D *)roverPosition
{
	return &robot.position;
}

- (Vector3D *)roverHeading
{
	heading.x = sin(DEGTORAD(robot.compass));
	heading.y = cos(DEGTORAD(robot.compass));
	heading.z = 0.0;
	return &heading;
}

- (NSString *)stateString
{
	NSString *stateString;
	
	switch (robot.state) {
		case RSStopped:
			stateString = [NSString stringWithString:@"Rover Stopped"];
			break;
		case RSMovingTowardsWaypoint:
			stateString = [NSString stringWithString:@"Moving toward Waypoint"];
			break;
		case RSNoRemainingWaypoints:
			stateString = [NSString stringWithString:@"End of Waypoint list"];
			break;
		case RSGoingToSleep:
			stateString = [NSString stringWithString:@"Going to Sleep"];
			break;
		case RSWakingFromSleep:
			stateString = [NSString stringWithString:@"Waking from Sleep"];
			break;
		case RSDoingScience:
			stateString = [NSString stringWithString:@"Doing Science"];
			break;
		case RSCallingForHelp:
			stateString = [NSString stringWithString:@"Calling for Help"];
			break;
		case RSInTeleopMode:
			stateString = [NSString stringWithString:@"Joystick Mode"];
			break;
		case RSAvoidingNearbyObstacles:
			stateString = [NSString stringWithString:@"Avoiding Near Obstacles"];
			break;
		case RSAvoidingDistantObstacles:
			stateString = [NSString stringWithString:@"Avoiding Far Obstacles"];
			break;
		case RSReachedWaypoint:
			stateString = [NSString stringWithString:@"Waypoint Reached"];
			NSBeep();
			break;
		case RSGoingPastObstacles:
			stateString = [NSString stringWithString:@"Driving past Obstacles"];
			break;
		default:
			stateString = [NSString stringWithString:@"Unknown State"];
			break;
	}
	return stateString;
}

- (NSString*)errorString
{
	NSString *returnString;
	
	switch (robot.error) {
		case RENone:
			returnString = [NSString stringWithString:@"-"];
			break;
		case REPosition:
			returnString = [NSString stringWithString:@"ERROR Rover position"];
			break;
		case REStall:
			returnString = [NSString stringWithString:@"ERROR Turn Stall"];
			break;
		case REProgress:
			returnString = [NSString stringWithString:@"Help not making progress"];
			break;
		case REPitch:
			returnString = [NSString stringWithString:@"ERROR pitch"];
			break;
		case RERoll:
			returnString = [NSString stringWithString:@"ERROR roll"];
			break;
		default:
			returnString = [NSString stringWithFormat:@"%d",robot.error];
			break;
	}
	return returnString;
}

- (NSString*)turnString
{
	NSString *returnString;
	
	switch (robot.turnAvoidDirection) {
		case -1:
			returnString = [NSString stringWithString:@"Turning Left"];
			break;
		case 1:
			returnString = [NSString stringWithString:@"Turning Right"];
			break;
		default:
			returnString = [NSString stringWithString:@"Straight"];
			break;
	}
	return returnString;
}

- (laserScanner*)bodyLaser
{
	return &bodyLaser;
}
- (laserScanner*)panelLaser
{
	return &panelLaser;
}
- (laserScanner*)profileLaser
{
	return &profileLaser;
}

- (void)configLaser:(laserScanner*)lazer
{
	int i;
	int *oldData = lazer->rangeData;
	int *newData;
	
	//NSLog(@"%d,%d,%d",lazer->startPoint,lazer->stopPoint,lazer->grouping);
	//NSLog(@"%d,%f,%f",lazer->rayCount,lazer->alpha,lazer->dAlpha);
	
	newData = malloc(sizeof(int)*lazer->rayCount);
	for(i=0;i<lazer->rayCount;i++) newData[i]=2000;
	lazer->rangeData = newData;
	if(oldData) free(oldData);
}

- (void)testLaser:(laserScanner*)scanner type:(SenseType)t
{
	if(t == SenseBodyLaser){
		scanner->startPoint = 218;
		scanner->stopPoint = 571;
		scanner->grouping = 22;
		scanner->rayCount = 16;
	}
	else if(t == SensePanelLaser){
		scanner->startPoint = 218;
		scanner->stopPoint = 571;
		scanner->grouping = 22;
		scanner->rayCount = 16;
	}
	else if(t == SenseProfileLaser){
		scanner->startPoint = 384;
		scanner->stopPoint = 640;
		scanner->grouping = 16;
		scanner->rayCount = 16;
	}
	scanner->dAlpha = ((135.0/384.0)*(float)(scanner->stopPoint-scanner->startPoint)/scanner->rayCount)*PI/180;
	scanner->alpha = ((135.0/384.0*(float)scanner->startPoint)-45)*PI/180;
	[self configLaser:scanner];
}

@end
